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RESUME 


Several  computer-oriented  methods  for  the  formulation  of  the 
equations  of  motion  for  multibody  systems  with  tree  structure  and  also 
with  closed  chains  are  presented  in  a  unified  manner.  They  all  stem  from 
the  same  basic  set  of  equations  which  are  regarded  as  arising  out  of  the 
stationary  point  condition  of  a  quadratic  programming  (QP)  problem. 
Using  QP  theory  it  is  shown  that  the  various  methods  are  just  different 
ways  of  solving  the  Lagrangian  system  of  equations. 


\ 

\ 

\ 

RESUME 

On  presente  dans  un  format  homog&ne  plusieurs  m£thodes  mfor- 
matisles  pour  l'etablissement  des  Equations  du  mouvement  de  syst&mes  & 
plusieurs  corps  avec  structure  en  arbre  et  avec  chatnes  fermdes.  Ces 
mdthodes  ddcoulent  toutes  du  m§me  ensemble  d* equations  de  base  qui 
represented  la  condition  de  point  stationnaire  d'un  problfeme  de  program- 
mat  ion  quadrat lque  (PQ).  A  l'aide  de  la  th6one  de  la  PQ,  on  montre  que 
les  differences  m£thodes  sont  simplement  des  faqons  differences  de 
r£soudre  le  systbme  d' equations  de  Lagrange. 


(in) 


ABSTRACT 


Page 

(iii) 


ILLUSTRATIONS  (iv) 

APPENDICES  26 

SYMBOLS  (v) 

1 .  INTRODUCTION  1 

2.  DESCRIPTION  OF  THE  INTERCONNECTIONS  2 

3.  EQUATIONS  OF  MOTION  FOR  SYSTEMS  WITH  TREE  STRUCTURE  4 

3.1  Systems  with  Ball-and-Socket  Joints  6 

3.2  Systems  With  Ball -and- Socket  Joints,  Universal  Joints  16 
and  Pin  Joints 

4.  EQUATIONS  OF  MOTION  FOR  SYSTEMS  WITH  CLOSED  CHAINS  21 

5.  CONCLUSIONS  24 

6 .  REFERENCES  24 

DOCUMENTARY  PAGE 


ILLUSTRATIONS 


Figure 

la  A  6-body  system  with  tree  structure  2 

lb  System  graph  of  a  6-body  system  2 

2  Directed  tree  representation  of  the  6-body  system  2 

3a  Regular  labeling  3 

3b  An  arbitrary  labeling  3 

4  Free-body  diagram  of  body  i  4 

5  Vectors  depicting  the  constraint  at  hinge  j  7 

A  6-body  system  with  closed  chains 


(iv) 


6 


21 


Sjabol  Definition 

Ik  Body  i  (i*l,2,...n) 

Cjj  Elements  of  the  incidence  matrix  (i-1,2 . . . . . . . ,n-l) 

C  Incidence  matrix 

C  Incidence  matrix  for  closed  chains 

c 

* 

C  Incidence  matrix  due  to  cut  hinges 

C^  Centre  of  mass  of  body  i  (i*»l,2,...n) 

d. .  Vector  from  the  barycenter  of  b.  to  C. ( i*l ,2 , . . . ,n) 

11  11 

dik  Vector  from  the  barycenter  of  b^  to  hinge  point  j,  for  each 

body  k  connected  to  b^  directly  or  indirectly  at  hinge 
j  (i*l ,2, . . . ,n;  k*l,2,...n;  i*  k) 

D  Matrix  with  elements  d  (i*l ,2 , . . . ,n,  k“l,2 . n) 

ik 

E  nxn  unit  matrix 

n 

e.  First  column  of  matrix  E 

1  n 

f.  Force  at  hinge  point  j  ( j*l ,2 , . , . ,n-l) 

f  Force  vector  with  components  f 

j 

F^  Resultant  external  force  on  body  i,  acting  at  C^(i*l ,2, . . . ,n) 

F  Force  vector  with  components  F 

i 

gj  Torque  at  hinge  point  j  ( j»l ,2. . . ,n-l) 

g  Torque  vector  with  components  g 

j 

c 

g  Constraint  torque  vector 

(v) 


o  ci 


SYMBOLS  (cone1) 


i 

h. 

1 


Resultant  external  torque  on  body  i  (i*l,2 
Torque  vector  with  components 

Angular  momentum  of  body  i(i"l ,2 , . . . ,n) 


> 


n) 


h 

h. 

J 

I. 

1 

I 

K 


Angular  momentum  vector  with  components  h. 

Arc  j  or  joint  j  (j*l,2 . n-1) 

Inertia  matrix  of  body  i  about  its  centre  of  mass  ( i*l ,2 , . . . ,n) 

T 

Diagonal  matrix  with  elements 
System  inertia  matrix 


‘ij 


L 


m. 

l 


m 

M 

n 


n. 

J. 

J 

N 

0 

0 


w  •  ■ 

jk 


Vector  from  to  hinge  point  j  (i*l ,2 . ,n;  j«l,2 . n-1) 

Matrix  with  elments  c^ 

Mass  of  body  i  (i*l ,2 , . . . ,n) 

Total  mass  of  the  system 

Diagonal  matrix  with  elements  m^  or  m^ 

Number  of  rigid  bodies  in  the  system 

Number  of  rotational  degrees  of  freedom  at  joint  j(j*l,2 . n-1) 

n!  -  3-n. 

J  J 


Matrix  of  nonlinear  elements  (see  Eq.  (38)) 

Origin  of  an  inertial  frame 
Zero  matrix,  zero  vector 

Unit  vector  along  the  axis  of  rotation  at  joint  j(j»l,2. 


k-l,...,nj) 


n-1) ; 


Pi 

P. 

J 


P 


V 

*jl’’j2 

Qj 

Q 


Unit  vector  in  the  direction  of  translational  motion  at  joint  j 
Matrix  with  elements  Pjfc  ( j«l ,2 , . . . ,n-l) 


Diagonal  matrix  with  elements  P 


j 

Unit  vector  in  the  constraint  direction  (j*l,2 
Unit  vectors  perpindicular  to  p^ 

Matrix  with  elements  q^  (  j“l  ,2 , . . .  ,n-l) 


,n-l ^  k“ 1 , • • .n^ ) 


Diagonal  matrix  with  elements  Q. 

(vi) 


SYMBOLS  (Goat*) 

r.  Position  vector  of  C.  from  0  ( i“l ,2 , . . . ,n) 

i  1 

r,r,r  Vector  with  components  r^,  r^  end  r\  respectively 

R^  Transformation  matrix  connecting  the  ith  frame  to  the  inertial  frame 

(i*l  ,2 . n) 

S  Matrix  defined  by  (-e^  C) 

T  Transpose  of  a  matrix 

T  Inverse  of  matrix  S 

Tj  First  row  of  matrix  T 

T2  Last  (n-1)  rows  of  matrix  T 

u,  ,un,u^  Components  of  force  defined  in  Eq.  (10) 


1  An  n-vector  with  unit  elements 

n 


y.  ,y.  ,y.  Relative  angular  position,  velocity  and  acceleration  respectively, 
JK  JK  JK 

at  joint  j( j»l ,2 , . . . ,n-l;  k»l,...,n.) 

Vectors  with  components  y^iY-v  and  T'v  respectively  ( j*l ,2 . n-1) 

J  J  J  JK  JK  JK 

Y»Y»Y  Vectors  with  compoentns  y  j «  Y j  >  an<*  Yj  respectively 


6..  Kronecker  delta 

ij 

X  Vector  Lagrange  multiplier  (see  Eq.  (49) 

X  Vector  Lagrange  multiplier  (see  Eq.  (63) 

c 


U  Matrix  defined  in  Eq.  (26) 

U£  Absolute  angular  velocity  of  body  i  (i“l,2...,n) 


Absolute  angular  acceleration  of  body  i  ( i“l ,2, . . . ,n) 
u  Vector  with  components  R.^ 

Qj  Relative  angular  velocity  at  joint  j  ( j«l ,2 , . . .n-1) 

n  Vector  with  components 

.  First  derivative  with  respect  to  time  variable  t 

..  Second  derivative  with  respect  to  time  variable  t 


(vii) 


IE> 


SYMBOLS  (Coat') 


Equals  by  definition 
j+,j-  Initial  vertex  and  final  vertex  of  arc  respectively 

x  Cross  product 

j  Used  to  define  the  cross  product  (see  Eq.  (11) 

(  )  Inverse  of  a  matrix 

diag  Diagonal  matrix 


(viii ) 


EQUATIONS  OF  MOTION  FOR  RIGID  MJLTIBODY  SYSTEMS 


1 .  INTRODUCTION 

The  dynamic  behaviour  of  manipulators,  linkages  in  machines  and  human 
body  mechanism  can  be  studied  by  utilizing  the  equations  of  motion  derived  from 
rigid  body  dynamics.  The  general  principles  involved  in  formulating  these 
equations  have  been  known  since  the  days  of  Euler  (1707-1783),  d'Alembert 
(1717-1783)  and  Lagrange  (1726-1813).  The  equations  of  motion  are  complex  and 
highly  nonlinear  and  can  be  only  solved  with  the  aid  of  computers.  Most  of  the 
recent  work  [1-7]  is  therefore  devoted  to  the  methods  that  are  efficient  and 
general  enough  to  be  applicable  to  a  wide  variety  of  multibody  systems  with  a 
minimum  amount  of  preparatory  work. 

The  two  most  popular  methods  for  the  derivation  of  equations  of  motion 
are  the  Lagrange's  method  and  the  Newton-Euler ' s  method.  As  the  final  form  of 
the  equations  derived  by  the  Lagrange's  method  can  also  be  obtained  by  means  of 
Newton-Euler ' s  method,  it  is  only  the  latter  method  that  will  be  considered. 

In  the  Newton-Euler ' s  method,  equations  of  motion  are  written  for  each 
body  of  the  system  with  internal  reaction  forces  and  torques  appearing  as 
external  loads.  Using  graph  theory  concepts  to  describe  the  interconnections 
between  the  bodies  and  writing  the  equations  of  motion  in  a  vector-matrix  form 
it  becomes  evident  that  these  equations  can  be  viewed  as  a  stationary  point 
condition  of  a  quadratic  programming  problem  with  equality  constraints  provided 
we  replace  the  physical  components  of  the  hinge  reaction  forces/torques  with  the 
Lagrange  multipliers.  This  point  of  view  is  useful  since  the  theory  of 
quadratic  programming  can  be  immediately  applied  for  the  formulation  of  the 
equations  of  motions  and  many  seemingly  different  approaches  can  then  be 
presented  in  a  unified  manner.  In  the  literature  [8,9],  this  quadratic 
programming  problem  is  known  as  the  Gauss's  principle  of  least  constraint. 

The  remainder  of  the  report  is  divided  into  four  sections.  In 
Section  2,  we  present  some  concepts  from  graph  theory  and  introduce  the  notion 
of  incidence  matrix  to  describe  the  interconnections  between  the  bodies  of  a 
system  with  directed  tree  structure.  Equations  of  motion  are  formulated  for 
systems  with  tree  structure  in  Section  3,  first  for  the  case  when  the  hinges  are 
ball-and-socket  (spherical)  joints  and  then  for  the  case  when  the  hinges  are 
ball-and-socket  joints,  universal  joints  and  pin  joints.  Several  ways  of 
formulating  the  equations  of  motion  are  discussed.  They  all  stem  from  the  same 
set  of  equations  but  differ  in  the  way  the  Lagrangian  matrix  in  the  quadratic 
programming  problem  is  manipulated.  It  is  shown  that  equations  formulated  by 
the  Lagrange's  form  of  d'Alembert's  principle  [10]  can  also  be  formulated  by 
using  the  Gauss's  principle  of  least  constraint.  In  Section  4,  the  methods  of 
Section  3  are  extended  to  multibody  systems  with  closed  chains.  Conclusions  are 
discussed  in  Section  5  and  the  matrix  computational  procedures  necessary  for  the 
determination  of  linear  accelerations,  angular  accelerations,  and  constraint 
forces  and  torques  are  presented  in  the  Appendix. 


V 


2. 


DESCRIPTION  Of  THE  INTERCONNECTIONS 


Let  us  consider  a  system  of  n  rigid  bodies  connected  to  each  other 
by  n-1  hinges.  If  we  identify  the  bodies  with  the  vertices  and  the  hinges  with 
the  links,  such  a  structure,  in  the  language  of  graph  theory,  is  called  a 
nondirected  tree  [11].  Figs,  la  and  lb  respectively  show  a  6-body  system  and  its 
tree  graph. 


Fig.  la  A  6-body  system  with 
tree  structure 


Fig.  lb  System  grpah  of  a 
6-body  system 


In  studying  the  dynamics  of  multibody  systems  the  hinge  forces  and 
torques  act  on  two  contiguous  bodies  with  opposite  signs  and  hence  we  must 
specify  unambiguously  which  of  the  two  bodies  is  acted  upon  by  force/torque  with 
a  positive  sign  and  which  one  with  a  negative  sign.  This  means  that  to  describe 
the  interconnections  between  the  bodies  we  must  convert  a  nondirected  tree  to  a 
directed  graph  by  assigning  a  sense  of  direction.  To  be  specific  we  shall 
convert  the  nondirected  tree  into  a  directed  tree.  By  this  we  mean  a  directed 
graph  without  a  circuit  for  which  the  indegree  of  every  vertex  b^,  (i.e.  the 
number  of  arcs  which  have  bj  as  their  final  vertex)  except  one  is  unity:  the 
indegree  of  the  exceptional  vertex,  called  the  root  of  the  tree,  being  zero. 


A  nondirected  tree  can  be  converted  into  a  directed  tree  by 
arbitrarily  picking  any  vertex  as  the  root  and  choosing  directions  for  the  links 
so  that  there  is  a  pathbetween  the  root  and  every  other  vertex.  It  should  be 
noted  that  there  can  be  only  one  such  path.  Fig.  2  shows  a  directed  tree 
representation  of  the  system  shown  in  Fig.  lb. 


Fig.  2  Directed  tree  representation  of  the  6-body  system 


A 


-3- 


So  far  we  have  not  paid  any  particular  attention  to  the  labeling  of 
either  the  vertices  or  the  arcs.  The  numbers  were  assigned  in  an  arbitrary 
way.  We  now  describe  a  procedure  called  regular  labeling  that  produces  a  simple 
structure  for  the  incidence  matrix  (defined  below).  For  this,  label  the  root  as 

body  1  and  the  peripheral  vertices  the  highest-numbers  n,  n-1 .  The 

peripheral  vertices  are  those  vertices  which  have  no  arcs  emanating  from  them. 
The  arc  with  final  vertex  n  is  labeled  n-1  and  that  with  n-1  is  labeled  n-2  and 
so  on.  Now  all  vertices  and  arcs  that  have  been  labeled  are  removed  from  the 
tree  producing  new  peripheral  vertices.  The  procedure  is  then  repeated  until 
all  vertices  and  arcs  have  been  labeled.  The  regular  labeling  and  an  arbitrary 


A  directed  tree  is  conveniently  represented  by  an  nxn-1  incidence 
matrix  C  (of  rank  n-1)  whose  elements  cjj  are  defined  as  follows: 


cij 

*  1 

if  bj^  is 

the 

initial  vertex 

of  arc  hj 

cij 

-  -1 

if  bj  is 

the 

final  vertex 

of  arc  hj 

Ci  i 

-  0 

otherwise 

Since  each  arc  is  adjacent  to  exactly  two  vertices,  each  column  of  the  incidence 
matrix  contains  one  element  1  and  one  element  -1.  For  the  directed  trees  shown 
in  Figs.  3a  and  3b  the  incidence  matrices  Ca  andC^  are 


C  - 
a 


h2  h3  h4 
1  0  0 


0  1  1 

-10  0 
0-10 

0  0-1 

0  0  0 


(1) 


.4 
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Ic  should  be  noted  that  with  the  first  row  omitted  the  incidence  matrix  Ca 
corresponding  to  the  regular  labeling  is  upper  triangular  having  all  elements  on 
the  main  diagonal  as  -1,  idiile  C5  does  not  have  any  such  simple  structure. 

3.  EQUATIONS  OF  NOTION  FOR  SYSTEMS  WITH  TREE  STRUCTURE 

Consider  a  system  of  n  bodies  with  tree  structure.  Let  us  isolate  the 
ith  body  from  the  system.  The  forces  acting  on  the  body  are  the  external 

forces,  hinge  forces  and  the  inertia  forces.  As  regards  the  hinge  forces  and 

torques  we  adopt  the  following  sign  convention.  The  forces  and  torques  at  hinge 
j  are  taken  positive  on  that  vertex  which  forms  the  initial  vertex  of  arc  h ; . 

The  index  of  such  a  body  will  be  denoted  by  j+.  The  index  of  the  body  which 
is  the  final  vertex  of  arc  hj  will  be  denoted  by  j-.  For  example,  for  the 
system  of  Fig.  3a,  5+  3  2,5"  3  6  and  on  body  2  the  force  is  fj  and  torque 

g5  while  on  body  6  the  force  is  -fj  and  torque  -g5. 

Let  fcjj  denote  the  position  vector  from  centre  of  mass  Cj  of  body 
b^  to  the  hinge  j,  Fig.  4.  Position  vector  is,  of  course,  zero  if  hinge 


0 


Fig.  4  Free-body  diagram  of  body  i 
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j  is  not  located  on  body  i.  The  equations  of  motion  of  of  body  i. 

i*l ,2, . . .n, 

are: 

n-1 

m  r  -  F  + 

I  c 

f  (Newton's  Equations) 

(2) 

i  i  i 

j«l 

ij  j 

T 

n-1 

I  u>  +b>  Xl  U) 

-  R 

[G  +  £  c  (R  i  xf  +g  )]  (Euler's  equations) 

(3) 

i  i  i  i  i 

i 

i  j=l  ij  i  ij  j  j 

where  T  denotes  the  transpose  and 
m.  *  the  mass  of  body  i 

1^  *  the  inertia  matrix  of  body  i  about  its  centre  of  mass 

r.  *  the  position  vector  of  C.  from  an  inertially  fixed  point  0 

t-j  =  vector  from  to  hinge  j,  expressed  in  body  frame  i 

F  ,G  *  the  resultant  external  force  and  torque  acting  at  C  ,  expressed 
i  i  in  an  inertial  frame  i 

fj,gj  =  hinge  force  and  torque  at  joint  j,  expressed  in  an  inertial  frame 

*  the  absolute  angular  velocity  of  body  i,  expressed  in  body  frame  i 

R  *  the  transformation  matrix  connecting  the  ith  frame  to  the  inertial 
i  frame. 

In  the  vector-matrix  notation  the  equations  of  motion  can  be  expressed  as: 

M  r  ■  F  +  Cf  (4) 

h  -  G  +  Lxf  +  Cg  (5) 

where  the  various  vectors  and  matrices  are  defined  as  follows: 

M  ■  diagonal  matrix  with  diagonal  elements  hk  or  (E^^xS  unit  matrix) 

r  *  (r’j  .r'g ». . .  ,r  )  »  F,G  defined  in  the  same  way 
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T 

f  -  (fltf2,...,fn_j)  ;  g  defined  in  the  same  way 

C  ■  nxn-1  matrix  with  elements  c..  or  3nx3(n-l)  matrix  with  elements  c..E» 

1 J  1 J 

L  *  nxn-1  matrix  with  elements  L.  .  **  c..R.£.. 

T1J  *■  *J 

h  *  (h^.hj, . h^)  an  n-vector  with  the  ith  element 


h  ■  R  (I  m  +  (j  x  I  u  ) 
i  i  i  i  i  i  i 

It  should  be  noted  that  the  elements  of  the  matrix  L  and  the  vectors  r,  f,  g,  F, 
G  and  h  are  3x1  matrices,  while  the  elements  of  M  and  C  can  be  taken  either  as 
scalars  or  3x3  matrices. 

Eqs.  (4)  and  (5)  can  be  used  to  describe  the  motion  of  the  system 
provided  the  forces  and  torques  at  the  couplings  between  two  neighbouring  bodies 
are  known.  Such  is  not  the  case  when  the  hinges  are  ball-and-socket  (spherical) 
joints  giving  arise  to  constraint  forces  that  must  be  determined  or  eliminated. 
In  case  the  hinges  are  universal  joints  or  pin  joints  then  in  addition  to  the 
constraint  forces  there  are  also  constraint  torques. 


3.1  Systems  With  Ball -and- Socket  Joints 

Let  us  first  consider  the  simplest  case  when  all  hinges  are 
ball-and-socket  joints.  The  relative  motion  of  any  two  neighbouring  bodies  is  a 
pure  rotation  with  three  degrees  of  freedom.  From  Fig.  5,  the  constraint 
equation  at  hinge  j  is  given  by: 


r.  +  R.  £ . ,  .* 
J+  J+  J+.J 


r.  +R.  £. 
J-  J"  J' 


j*l ,2, . . . ,n-l 


or 


c .  .  r .  +c .  .  r .  ♦ 

J+»J  J+  J-.J  J- 


c .  . +c .  .R.  A. 

J+»J  J+  J+»J  J-.J  3~  3~»3 


0 


or 

n  n 

l  c  r  *  l  c  R  £  -0 

1*1  ij  i  i*l  ij  i  ij 

since  for  each  j,  c.  .*1,  c.  .*-1  and  c..*0,  i#j+  or  j-. 
J+.J  J-.J  iJ 


(6) 
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^  the  ijth  element  of  L  -  R-(u>x(u.xc. ji^. )) 

A  A 

h^  ■  the  ith  component  of  h  *  R^oj^xI^oi^) 

I  -  diag  {R1I1Rp...,RnIn**} 

(I)  *  f  •  •  • 

L  *  nxn-1  matrix  with  elements  L..t  i*lf*..,n>  j*l . n-1,  and  the 

tilde  (")  on  a  vector  r  with  components  x,y,  and  z  denotes  a  3x3  antisysmmetric 
matrix 

•  l""\ 

r  -  I  z  0  -x  (11) 

\  -y  x  0  / 

so  that  r^xr2  *  *1*2'  E<*‘  represents  6n  ♦  3(n-l)  scalar  equations  for  the 

determination  of  the  linear  accelerat ions ,  angular  accelerations  and  the 
constraint  forces.  It  should  be  noted  that  the  matrix  representing  Eq.  (10)  is 
sparse  (also  symmetric)  and  hence  sparse  matrix  techniques  can  be  used  to  invert 
this  matrix.  We  can  also  reduce  the  order  of  the  matrix  inversion  problem 
through  some  preliminary  analytic  manipulations. 

From  Eq.  (10)  we  see  that  if  f  is  known  then  r  and  can  be  determined 
easily  by  inverting  the  diagonal  matrices  M  and  I: 

r  -  M_1 (uj+Cf )  (12) 

iL  -  I-1(u2+Lf)  (13) 

T  -1 

Now,  to  determine  f  multiply  the  first  row  by  C  M  ,  the  second  row  by 


(3  f 1  and  add  to  the  third.  This  yields 

T  -1  ~  t  - 1  ~  T  -1  ~  T  -1 

-(C  M  C  +  (L)  I  L)f  -CM  Uj+U)  I  u2+u3 

The  3(n-l)x3(n-l)  matrix  representing  Eq.  (14)  can  also  be  written  as 


T  “  T  M  0  C 

-(c  a>  Ho  i-0  (£  ) 


(14) 
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and  can  be  inverted  r  merically  (see  Appendix)  and  f  determined.  Having  found 
f,  the  linear  and  a  lar  positions  and  velocities  can  be  computed  from  Eqs. 
(12)  and  (13)  by  integration.  We  mention  that  only  the  first  equation  of  Eq. 
(12)  needs  to  be  integrated  since  if  the  position  vector  r^  is  known  the  others 

can  be  determined  from  Eq.  (7).  Also  knowing  r  ,  the  remaining  r.  can  be 

T  •  •  T  1  1 

determined  from  the  equation  C  r  +  L  1  •  0. 


It  should  be  noted  that  the  above  mentioned  procedure  requires  an 
inversion  of  a  3(n-l)x3(n-l)  matrix  (Eq.  (14))  and  the  inversions  of  n,  3x3 
matrices  (Eq.  (13)). 


We  now  discuss  another  matrix  reduction  procedure.  We  first  note  that 
the  second  equation  in  Eq.  (10)  does  not  contain  r  term.  So  if  we  eliminate  r 
term  from  the  third  equation  we  obt^in^equations  in  f  and  m.  For  this  multiply 

the  first  equation  of  Eq.  (10)  by  C  M  and  add  it  to  the  third.  This  yields 


( 


-L  1 

T  -1 
-CM  C 


(15) 


At  this  stage  we  can  either  solve  Eq.  (15)  numerically  for  oj  and  f  or  reduce  the 
dimensionality  of  the  matrix  further.  Eliminating  f  we  obtain 


(I  +  L  (CTM-1C)_1(L)T)i  *  -L(CTm“1C)'1(CTm"1uj+u3)+u2 


(16) 


T  -1 

Eq.  (16)  requires  the  inversion  of  the  matrix  CMC,  which  can  be  obtained 
either  numrically  or  analytically.  Numerical  inversion  in  the  context  of 
systems  with  tree  structure  having  only  one  branch  is  discussed  in  [12].  We 
note  that  the  matrix  in  this  case  is  tridiagonal. 


To  obtain  the  matrix  inversion  analytically  consider  the  matrix 


This  matrix  is  a  submatrix  of  the  matrix  representing  Eq.  (10)  and  can  be 
thought  of  as  arising  from  the  optimization  problem: 

min  -  (r-M“1F)TM(r-M"1F) 
r  2 


subject  to  the  constraints 


CTf  ♦  LT1 


0 


T 
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This  is  nothing  else  but  a  quadratic  programing  problem  with  equality 
constraints  and  the  inverse  of  the  Lagrangian  matrix  (17)  can  be  written  in  the 
following  two  alternative  forms  (see  for  example,  [13]): 


",  ^  )“  -  ( "■* ' M" 

-C  0  /  V  -B 

”,  ■*)"./  ■ 

-C  0  /  l  TjME  -T2 

where 

B  -  A  CTM_1 

A  -  (CTH-1C)'1 

T  T  -1 

E  -  T. 

and  Tx  is  a  1  x  n  matrix  such  that  TxOO  and  T2  is  an  n-lxn  ipatrix  such  that 
T^OE^j,  the  unit  n-lxn-1  matrix.  In  addition,  matrix  T>  (^l)  is 

nonsingular.  Comparing  Eqs.  (18)  and  (19)  we  obtain 


CB  -M  CA 
-A 


(18) 


T  T 

emt2-t2 


t2meht2 


'V  / 


(19) 


B  •  T2  -  T2ME 

T  T 

A  -  -(T2MEKT2  -  T2KT2) 


(20) 

(21) 


Thus  A  and  B  can  be  determined  analytically  provided  the  matrices  1 l  and  T2  are 
known. 

A  general  method  for  computing  Tx  and  Tz  is  to  augment  the  matrix  C  by 
adding  a  column  such  that  the  resulting  matrix  is  nonsingular  (see  Appendix). 

In  the  present  case  we  can  take  the  following  matrix  as  the  augmented  matrix 

S  -  (-ej  C)  (22) 

T 

where  *  (1  0  0  ...  0)  .  The  matrix  S  is  nonsingular  since  it  is  upper 

triangular  with  -1  elements  along  the  main  diagonal.  Let  its  inverse  be  denoted 
by  T  and  represented  as  a  partitioned  matrix 


(23) 
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Then  from  the  definition  of  the  inverse  we  have  T^OO,  TjOE^j,  T^-l  end 

T^e^-O.  Since  each  column  of  C  contains  on£  element  1  and  one  element  -1  it 

follows  from  T.OO  and  T,  e.*-l,  that  T,  “  -1  where  1  is  a  column  matrix  of  n 
1  11’  In  n 

elements,  each  equal  to  1, 

To  determine  T£,  first  consider  T2eA"0.  Performing  the  matrix 
multiplication  we  see  that  the  first  column  of  T2  is  zero.  Mow  using  T^OE^j 

we  obtain  for  each  j  ( j“l ,2, . . . ,n-l) 


-  V  lM— --1 


(24) 


where  is  the  Kronecker  symbol  and  are  the  elements  of  Tj. 


Matrix  T  has  a  simple  graph-theoretical  interpretation  [2].  Imagine  a 

fictitious  body  b  attached  to  the  root  b,  of  the  directed  tree  such  that  b  is 
O  X  o 

the  initial  vertex  of  the  arc  hQ  connecting  bQ  to  .  The  elements 
t.  ,(k**0,l , . . .  ,n-l;  g»l  ,2 . n),  of  T  are  obtained  from  the  relations 

t,  *  -1  if  arc  h,  is  on  the  path  between  b  and  b 
kt  k  o  t 

m  0  otherwise 

For  the  case  of  a  multibody  system  represented  by  the  graph  in  Fig.  3a 

we  have 


T  - 


-1 

-1 

-1 

-1 

-1 

-1 

0 

1-1 

0 

-1 

-1 

-1 

0 

.  0 

-1 

0 

0 

0 

0 

'  0 

0 

-1 

0 

0 

0 

>  0 

0 

0 

-1 

0 

V  o 

1  0 

0 

0 

0 

-1 

(25) 


Notice  that  matrix  T  is  an  upper  triangular  (since  S  is)  with  -1  as  diagonal 
elements. 


obtain 


Substituting  the  value  of  T1  into  the  expression  for  E,  B  and  A  we 


E  “  i  1n1n’  B  *  V’  A  “  WT1 


(26) 


IT  T  T 

where  m  is  the  total  mass  of  the  system  and  y  »  Eq-  —  Ml nl n  •  Since  yMy  "My 
the  matrix  A  can  also  be  written  as 

A  -  T,yMyV  -  T,yM(T,y)T 


T 

-  B  M  B 


(27) 
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Using  Eq.  (27),  Eq.  (16)  can  be  written  as 
(1  ♦  D  M(D)T)  <i  ■  D  ttj  ♦  Uj  +  0  M  BTu3  (28) 

%ihere  the  nxn  matrix  D  -  -LB  -  -LT^u*  Substituting  the  values  of  u^u^tU^ 
into  Eq.  (28),  the  rotational  equations  can  be  written  as 

m  m  —  m  a  a 

(l  ♦  DM(D)T)  U  -  DF  -  DM(D)T  1  -  h  ♦  G  +  Cg  (29) 

*  A 

where  D  is  defined  in  the  same  way  as  L. 

Matrix  D  introduced  above  has  a  simple  physical  interpretation. 

Augment  the  mass  of  each  body  i  by  placing  point  masses  at  each  hinge  on  the 
body,  the  mass  at  hinge  j  being  the  sum  of  masses  of  all  bodies  connected  to 
body  i  at  hinge  j  either  directly  or  indirectly.  As  an  example,  for  the  system 
shown  in  Fig.  3a,  the  augmented  body  2  is  obtained  by  placing  the  masses  m  -Hn^y 
m^,  m^  and  m6  at  joints  1,3,4  and  5  respectively.  It  is  clear  from  the 

definition  that  all  augmented  bodies  have  the  same  mass  m,  the  total  mass  of  the 

system.  The  centre  of  mass  of  the  augmented  body  is  called  the  barycenter  [1,2] 
of  the  body.  The  diagonal  elements  d„  is  the  vector  from  the  barycenter  of 

body  i  to  the  original  centre  of  mass  while  for  each  body  k  connected  to  body 

i,  directly  or  indirectly  at  joint  j,  d^  is  the  vector  from  the  barycenter  to 

joint  j.  It  is  quite  clear  that  d^  are  not  all  different.  For  example 

(Fit.  3.),  d12  -  dH  -  d15  -  d16. 

To  obtain  some  insight  into  the  various  terms  involved  in  Eq.  (29)  let 
"  T  •  . 

us  evaluate  the  matrix  DM(D)  .  The  ith  diagonal  element  is  given  by 

-  ^  m  d  d  (30) 

k-1  k  ik  ik 

which  simply  represents  the  inertia  matrix  of  the  point  masses  of  the  augmented 

-  -  T 

body  i  about  its  barycenter.  This  means  that  the  diagonal  elements  of  I+DM(D) 
represent  the  inertia  matrices ^of^the  augmented  bodies.  Similarly  the  ijth  term 
of  the  matrix  DM(D)T*  -  £  m  d  d  Using  the  properties  of  d  it  can  be 

k-1  k  ik  jk  ik 

shown  [2]  that 


l  m  d  d 
k-1  k  ik  jl 


md  d 

ij  ji 


where  j  here  refers  to  the  index  of  the  body. 

-  A  T 

We  now  simplify  the  expressions  appearing  in  the  matrix  -DM(D)  . 
The  ith  diagonal  element  of  this  matrix  is 


Vmdd  •  -  [  ad  (R  u  x(R  ot  xd 
k-1  k  ik  ik  k*l  k  ik  i  i  i  i  it 


(32) 


-13- 


Using  the  identity  a  x  (bx(bxc))  •  -bcab  +  ca  b1b,  Eq.  (32)  can  be 
written  as 

M  *  m  «  « 

-  J  ■  d  d  *(R  u  )  m  d  d  )S  u  (3' 

k"I  k  ik  ik  i  i  k«l  k  ik  ik  i  i 

Similarly  the  ijth  element  of  the  matrix  is 

-  f  ad  d  *  (R  o>  )  (^  md  d  )R  «  -  (J  md  d  )u?u> 
k*l  k  ik  jk  j  j  k“l  k  jk  ik  j  j  k*l  k  jk  ik  j  j 

•  •  *  -  t  ( 3^ 

*  -m(R.w)  d..d..R.w  +  m  d..d..w  w 
J  J  Ji  J  J  Ji  ij  J  J 

-  -  x 

Let  K  denote  the  positive  definite  matrix  I  +  DM(D)  .  Then  from  Eqs. 
(30)  and  (31)  we  have 

T  n  „ 

K  *  R  I  R  -  ][  md  d 
ii  i  i  i  k“l  k  ik  ik 


K.  .  »  m  d. .  d.. 

ij  Ji 


i  *  j 


With  this  notation,  Eqs.  (33)  and  (34)  can  be  written  as 

n  „  .  -  T 

-  I  mdd  «  (R  u  )(R  I  R  -  R  )R  u 
k"l  k  ik  ik  i  i  i  i  i  ii  i  i 

n  -  -  T 

-  £  m  d  d  ”  -(R  u>  )K  R  to  +  md  d  uco 

k-1  k  ik  jk  j  j  ji  j  j  ji  ij  j  j 
Substituting  these  values  into  Eq.  (29)  ve  get 

A  i  .  _ 

K«*-h+DF+G+Cg+Hl  u_ 

n  ■  2 

where 

“i  a  i  - 

h.  ■  the  ith  element  of  h  ■  (R.w)  K..(R.w) 

i  ii  ii  ii 

and  the  matrix  N  is  defined  as  follows 


"ij  *  0  l"i 


T 

M. .  ”  -(R.w  )  K.  .R.w  *  md  .  .d . . oj •  <u •  i*j 
J  J  Ji  3  J  Ji  lJ  j  J  J 


To  determine  u>  from  Eq.  (29)  or  Eq.  (38)  we  can  use  the  Cholesky's 
decomposition  method  for  solving  systems  of  linear  equations  with  positive 
definite  matrices.  It  should  be  noted  that  matrix  K  is  not  sparse. 
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Remark  1  It  was  mentioned  above  that  the  first  and  third  equation  of  Eq.  (10) 
can  be  obtained  from  an  optimization  problem.  In  fact,  by  using  Lagrange 
multipliers  method  it  can  be  seen  that  Eq.  (10)  is  the  stationary  point 
condition  of  the  following  optimization  problem: 

min  {  I  (r  -  M”lF)TM(r-M""1F)  -*■  —  (a»  -  T^G+Cg  -  h))TI(&-l”l(G+Cg-h) }  (39) 

*■'»&  2  2 

T.*  ~T 

subject  to  the  constraints  C  r  +  L  1  »  0 

n 

°r  CTr  ♦  (L)Ti  -  — (L)T1 

n 

This  minimization  problem  is  the  statement  of  a  principle  known  as  Gauss's 
principle  of  least  constraint  [8,9]  and  can  be  exploited  for  the  approximate 
determination  of  constraint  forces  (vector  Lagrange  multipliers)  by  using 
penalty  function  methods  [14]. 

Remark  2  Equations  of  motion  can  also  be  obtained  without  the  introduction  of 
Lagrange  multipliers.  Let  q  denote  the  (n+1)  vector  representing  the 
independent  variables  for  the  optimization  problem.  Differentiating  the 
optimizing  function  partially  with  regard  to  q,  the  stationary  point  condition 
is 


T 

)  (Mr-  Uj)  ♦( 


3u> 

3T 


T  . 

)  (I<|>  -  Uj) 


0 


As  t  and  m  can  be  taken  as  a  linear  combination  of  q  it  follows  that 
3r  3r  3u  3u 

—  *  —  and  —  *  —  .  Substituting  these  into  the  above  equation  we  obtain 
3q  3q  3q  3q 

(1L)T  (m  r-  u.)  ♦  (~  )T  (ii-u.)  -  0  (40-a) 

3q  3q 

Eq.  (40-a)  is  a  vector-matrix  formulation  of  the  so-called  Lagrange's  form  of 
d'Alembert's  principle  [10].  Eq.  (40-a)  can  also  be  written  as 


(AT  *T)  [( 

3q  3q 


M  0 
0  I 


)i-° 


(40-b) 


We  now  establish  the  connection  between  the  equations  of  motion 
d^rived^by^ using  Eq.  (40-b)  and  the  Lagrange  multipliers  method.  For  this  let 

Z  •  (Z ^  ,Z^)  denote  a  matrix  such  that 
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L 

<C  \  T 

£  Jis  of  full  rank  3(n-l),  a  nonzero  Z  of  rank  3(n+l)  exists. 

T 

Multiplying  the  first  two  equations  of  Eq.  (10)  by  Z  we  obtain 


(40-c) 


As  Eqs.  (40-b)  and  (40-c)  are  both  independent  of  the  Lagrange  multipliers  we 

can  identify  —  with  Z.  and  with  Z0 .  Thus  to  apply  the  Lagrange's  form  of 

d'Alembert's  principle  all  we"need  is  to  determine  a  matrix  Z  .  As  an 
example,  we  can  take 


ln  0  \  (40-i 

-^T2  En  / 

which  corresponds  to  the  choice  of  taking  q^=r ^ ,q =to^ C i=l . n) .  This  can 

~  T 

be  easily  seen  since  r  =  r^l  -  (LT  )  to.  Eliminating  r  by  using  Eq.  (19)  it 

can  be  seen  that  the  rotational  equations  are  the  same  as  those  given  by  Eq. 
(28).  It  should  be  noted  that  in  the  language  of  linear  algebra  the  space 
generated  by  the  columns  of  Z  forms  the  orthogonal  complement  of  the  space 


generated  by  the  columns  of 


Remark  3.  In  certain  applications  a  multibody  system  is  connected  to  an 
external  body  whose  motion  is  prescribed.  For  example,  a  manipulator  attached 
to  a  moving  platform  or  to  the  ground.  In  such  cases  the  above  analysis  can  be 
easily  modified. 


We  assume,  without  loss  of  generality,  that  the  system  is  connected  to 
the  external  body  through  a  single  hinge.  For  if  there  are  more  than  one 
hinges,  then  the  system  can  be  broken  into  several  dynamically  independent 
systems.  Label  the  external  body  as  0  and  the  body  to  which  it  is  attached  as 
1.  Let  hQ  denote  the  hinge  connecting  these  two  bodies.  Then  the  additional 
constraint  at  hinge  0  can  be  written  as 


rl  * 


(R  l 
ooo 


Vio* 


(41) 
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where  r0  is  the  vector  from  the  origin  of  the  inertial  frame  to  the  centre  of 
mass  of  body  o  and  t.^(i*0,  1)  are  the  vectors  from  to  hinge  0.  By  defining 

c^Q*  -1  for  i*l  and  zero  otherwise,  Eq.  (41)  can  also  be  rewritten  as 

in  n 

-e  r  *  1  c  R  t  “  -r 
1  1*1  io  i  io  o 

where  r  *  r  +R  i  .  Combining  this  equation  with  the  constraint  Eq.  (7)  we 
o  o  o  oo 

obtain 

T  -T  * 

S  r  +  LI  *  -r  e, 
n  o  1 

where 


L 


nxn  matrix  with  elements  L. .*  c..R.£..; 

ij  iJ  i  iJ 


i*l,,,,n,  j-0,1 


5n-l 


Equations  of  motion  can  therefore  be  obtained  by  replacing  C  with  S,  L  with  L 
and  by  adding  rQe^  to  the  right  hand  side  of  the  third  equation  in  Eq.  (10).  T 
S  is  nonsingular,  matrices  B  and  A  in  this  case  are  given  by  B  *  T  and  A  =  TMT  . 


3.2  Systems  with  Bal 1-aod-Socket  Joints,  Universal  Joints  and  Pin  Joints 

We  now  discuss  the  case  when  the  rotational  degrees  of  freedom  at  some 
joints  may  be  less  than  three  i.e.  the  hinges  may  be  universal  (two  degrees  of 
freedom)  or  pin  (one  degree  of  freedom)  joints.  Let  denote  the  relative 
angular  velocity  of  body  j"  with  respect  to  body  j+  expressed  in  the 
inertial  frame.  Then 


n 

$1  *  R  <i>  -  R  u>  *  \  cRu),  j*l ....  ,n-l  (42) 

j  j~  j“  j+  j+  1*1  ij  i  i 

In  terms  of  the  relative  angular  rates,  $)j  can  be  written  as 
nj  .  A 

£1  *  \  p  y  *  P  y  (43) 

j  k*l  jk  jk  j  j 

where  n:  *  1,2,  or  3  according  as  the  hinge  is  a  pin,  universal  or 
ball-ana-socket  joint  and  pjlc(k*l , . . . ,nj)  denote  the  unit  vectors  along  the 
axes  of  rotation  and  are  functions  of  tne  orientation  angles. 


To  obtain  the  rotational  cosntraint  conditions  we  denote  by 

q ..  (k*l , . . . ,n*  . )  the  unit  vectors  in  the  constraint  directions  where  n' . 
J*  J  J 


satisfies  n.+  n'.  ■  3.-  The  vectors  p.^  and  q.  are  mutually  orthogonal.  Since 
there  is  no^ relative  angular  velocit^  in  the^airection  of  the  constraint  axes  we 
have 


..  ****- 


can  be  rewritten  as 

fi  -  -CT(o  (45) 

T 

Q  II*  0  (46) 

Substituting  Eq.  (45)  into  Eq.  (46)  we  obtain  the  constraint  conditions 

QTCT.  -  0  (47) 

Equations  of  motion  can  now  be  written  down  as  the  stationary  point 
condition  of  the  optimization  problem  stated  in  Eq.  (39)  along  with  the 
additional  constraints 

T  T.  .T  T 

QCdi  +  QCu’O  (48) 


where  Eq.  (48)  is  obtained  by  differentiating  Eq.  (47)  with  respect  to  time. 

Introducing  the  vector  Lagrange  multiplier  X  *  (  X.  ,  X„  » . . .  ,  X  ,)  where  each 

1  l  n-1 


X.  is  an  n!  vector  we  have 
J  J 


/M 

0 


0 

I 


-c1  -(L) 


-C 

-L 

0 


\ 


T  T 

o  -q  c 


(49) 


.T  T 

where  Uj,u2,u3  are  the  same  as  in  Eq.  (10)  and  u^  =  Q  C  w  Matrix  represent¬ 
ing  Eq.  (49)  is  sparse  and  symmetric  and  hence  sparse  matrix  techniques  can  be 
used  for  solving  Eq.  (49).  As  in  the  case  of  equations  without  rotational 
constraints  some  preliminary  matrix  manipulations  may  be  useful.  For  example, 
corresponding  to  Eq.  (14)  we  have 


(  ctm_1c  +  (l)ti-1l) 

QTcVl£ 


(L)Tr1CQ 

qtcti-1cq 


T  -1  ”  T  -1 

CM  Uj  +  (L)  I  u2  +  u3 

T  T  -1 

QVl  u2  +  u4 


J 
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Several  ocher  possibilities  can  be  explored  for  Che  solution  of  Eq. 
(49).  As  an  example  let  us  eliminate  if,  f  from  Eq.  (49).  This  yields  (see  Eq. 
(38))  the  following  equation: 


Eq.  (51)  can  now  be  solved  numerically  using  some  factorization  method.  The 

n-1 

square  matrix  representing  Eq.  (51)  has  dimension  3n  +  V  n'  while  the  mat  .x 

J=1  j 

n-1 

representing  Eq.  (49)  has  dimension  6n  +  3(n-l)  +  T  n’.  However,  it  should 

J=1  j 

be  noted  that  matrix  in  Eq.  (51)  is  less  sparse  than  that  in  Eq.  (49). 

Rather  than  solving  Eq.  (51)  numerically  we  can  eliminate  X 
analytically  by  utilizing  the  condition  that  the  constraint  torque  along  the 
axis  of  rotation  is  zero.  From  Eq.  (51)  we  have 

.  _  c 

K<o  ■  u  ♦  C  g  (52) 

2 


where 

gC  -  QX  (53) 

is  the  constraint  torque  at  the  hinges.  Let  the  quasi-diagonal  matrix  P  be 
defined  as 

P  *  diag  {p1,...,pn_1}  (54) 


where  P j ,  the  jth  diagonal  element,  is  given  by  P^ 


(Pjl-. 


•  »P 


P  is 


n-1  T 

3(n-l)x  £  n  matrix  and  satisfies,  by  definition,  the  condition  P  Q  «  0. 

J™)  J  T  T 

Multiplying  Eq.  (53)  on  the  left  by  P  and  using  P  Q~0  we  obtain 

T  c 

P  g  -  0  (54) 


Eq.  (54)  is  just  a  mathematical  representation  of  the  fact  that  the  constraint 
torques  along  the  axes  of  rotation  are  zero. 


c 

To  utilize  Eq.  (54)  we  need  to  know  the  expression  for  g  .  For  this 
multiply  Eq.  (52)  on  the  left  by  the  matrix  T.  This  yields 


TjKi  -  rYu2  (55) 

a  —  c 

T2*<i>  ■  T2U2  ♦  g 


(56) 
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si  nee  T.OO,  T„OE  ,  .  To  eliminate  Che  constraint  torque  from  Eq.  (56)  we 
l  4  n-i 

multiply  it  on  the  left  by  P  and  obtain 

ptt2k«  -  ptt2u2 


(57) 


All  that  remains  to  be  done  is  to  express  Eqs.  (55)  and  (57)  in  terms  of  the 
relative  joint  angles.  Substituting  Eq.  (43)  into  Eq.  (45)  and  using  the 
definition  of  P  we  have 

-CTU  -  P  y  <58) 

T 

Multiplication  on  the  left  by  yields 

-(CT2)Tw  -  T2P  y  (59) 

T  .... 

Since  CT„  *  E  -  e.l  we  obtain  after  some  simplifications 
2  n  1  n 

“  *  Vn  “  T2  P  V  (60) 


By  substituting  Eq.  (60)  into  Eqs.  (55)  and  (57)  we  obtain  the  rotational 
equations  of  motion: 

T  T  t-  f  T  •  • 

lnUJ*l  “  1nKT2PV  “  W  *n  CT2^ 

T  .  T  T  T  T.. 

-P  TjKl^j  ♦  P  T2KT2PV  -  -P  T2(u2  ♦  KT2PY) 


(61) 


n-1 

Eq.  (61)  represents  3  +  £  n  scalar  equations  corresponding  to  as  many 

j 

rotational  degrees  of  freedom. 

Remark  4.  When  the  linear  or  angular  velocities  are  determined  by  integrating  a 
system  of  equations  greater  than  the  number  of  degrees  of  freedom  then  it  is 
advisable  [9,15]  to  replace  the  constraint  Eqs.  (8)  and  (48)  by  equations  which 
are  asymptotically  stable.  Consider,  as  an  example,  the  constraint  Eq.  (48). 

If  id  is  chosen  according  to 

ATT 

v  ■  Q  C  ui  ■  0 


then  v*0  (Eq.  (48)),  implies,  at  least  theoretically,  that  v  ■  0  for  all  time 
t.  However,  due  to  unavoidable  numerical  errors  during  the  computation  the 
equation  v  *  0  will  not  yield  v"0  for  all  t.  To  correct  this  situation  we 
replace  vs0  by  v  ♦  av  ■  0  (a  >0)  which  is  asymptotically  stable  so  that  any 
error  in  v  tends  to  zero  with  the  passage  of  time. 


i 
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In  a  similar  manner  Eq.  (8)  should  be  replaced  by 
i  *  o$  +  85  *  0  (a, 6  0) 
where  £  •  CTr  +  Z  1^ 


Remark  5.  The  derivation  of  equations  of  motion  have  been  discussed  in 
connection  with  hinges  that  allow  only  rotational  degrees  of  freedom  between 
neighbouring  bodies.  This  is  not  a  restriction  and  the  procedures  can  be 
extended  in  an  obvious  manner  to  other  types  of  joints  which  also  have 
translational  degrees  of  freedom.  As  an  example,  consider  the  case  of  prismatic 


joint  allowing  translational  motion  along  a  unit  axis  p  .  We  can  now  find  two 

-  -t  J  -T  - 

mutually  perpendicular  directions  1  j 2  8UC^  c^at  q^jPj*  c'j2pj*®'  Writing 
^  n 

sp»-  [  (c  r+L  )  where  s  is  the  distance  along  p  from  a  point  in 
j  j  l-l  ij  i  ij  j  j 

body  j  +  ,  we  obtain 


T  n 


q  (V  (c  r  +  L  )  -  0 
jl  l-l  ij  i  ij 

_T  n 

q  (£  (c  r+L  ))  -  0 
j2  l-l  ij  i  ij 

These  two  equations  should  therefore  replace  the  three  scalar  equations 
n 

£  (c  r+L  )-0  in  forming  the  translational  cosntraint  equations.  Thus 

i-1  ij  i  ij 

instead  of  Eq.  (7)  we  have 


QT(CTr+LTl  ) 
n 


0 


where  Q  -  diag  {(^ . Q^}  and  -  (q^.q^). 

degrees  of  freedom  we  have  ft,-0  i.e.  n.-O  and  Q. 


Since  there  is  no  rotational 
in  Eq.  (44)  can  be  taken  as  E^ . 
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4.  EQCJATIORS  or  (CTIOH  fOR  SYSTEMS  WITH  CLOSED  CHAIRS 

We  now  consider  multibody  systems  with  closed  chains,  that  is,  systems 
of  n  rigid  bodies  with  (n-l)+n*  hinges,  where  n*  is  a  positive  integer.  Fig.  6 
illustrates  a  6-body  system  with  7  hinges.  A  closed  chain  system  can  be 


Fig.  6  A  6-body  system  with  closed  chains 

converted  into  a  system  with  tree  structure  in  several  ways  by  removing  n* 
hinges.  For  example,  for  the  system  shown  in  Fig.  6  if  we  remove  the  hinges 
between  bodies  1  and  5,  and  bodies  4  and  6  we  obtain  a  system  with  tree 
structure. 


To  describe  the  interconnection  structure  for  systems  in  closed  chains 
we  first  determine  the  incidence  matrix  C  for  the  system  with  tree  structure  by 
cutting  n  hinghes.  The  cut  hinges  are  now  labeled  in  an  arbitrary  order  from 
n  to  (n-l)+n*  and  the  arc  direction  chosen  arbitrarily.  In  this  way  the 
nx(n-l)+n*  incidence  matrix  Cc  for  the  closed  chain  can  be  written  as 

* 

C  -  (C,C  )  (62) 

c 

where  the  nxn-1  matrix  C  represents  the  incidence  matrix  for  the  tree  structure 
and  the  nxn*  matrix  C*  represents  the  contribution  from  the  cut  hinges.  For 
the  system  shown  in  Figure  6,  the  dimensions  of  the  matrices  Cc,  C  and  C 
respectively  are  6x7,  6x5  and  6x2.  It  should  be  noted  that  the  rank  of  the 
incidence  matrix  Cc  is  n-1,  the  same  as  that  of  C. 

Equations  of  motion  can  be  written  down  in  exactly  the  same  manner  as 
is  done  for  the  case  of  systems  with  tree  structure.  For  example,  in  the  case 
of  ball-and-socket  joints  we  have 


A 

*•”  V(il . is  a  vector  Lagrange  multiplier  with  each 

A^*  3-vector.  The  Lagranian  matrix  in  Eq.  (63)  is  nonsingular  if  and  only  if 
the  aatrix  H, 


is  of  rank  3(n-l+n  ).  If  rank  of  H  is  3(n-l+n*)  we  use  the  procedures 
discussed  in  Section  3  to  obtain  the  final  fora  of  the  equations  of  notion. 
Otherwise  we  must  use  those  procedures  (see  Appendix)  which  do  not  require  H  to 
be  of  rank  3(n-l*n*) .  Note  that  the  rank  deficiency  implies  that  the 
constraints  are  not  independent. 

Ue  can  also  make  use  of  the  results  of  Section  3  in  another  way  by 
portioning  the  constraint  equation  in  two  parts,  one  corresponding  to  the  tree 
structure  and  the  other  due  to  the  cut  hinges  and  writing  only  the  equations  of 
motion  for  the  tree  structure.  Let  the  constraint  equation  be  written  as 

CTr  ♦  LTln  -  0  (65] 

*T  *T 

C  r  ♦  L  1  -  0  (661 


where  the  unstarred  Eq.  (65)  corresponds  to  the  constraints  for  the  system 
reduced  to  tree  structure.  Using  the  constraint  Eq.  (65)  we  can  write  the 
equations  of  motion  as 


C  °)( 

\0  I/\(i/  \L  /  \xx2 


Eliminating  X  by  multiplying  on  the  left  by  the  matrix  Z  (see  Remark  2). 


-LT2  En 


obtain  a  system  of  (n+1)  vector  equations: 


1TM  0 
n 


-lt2m  I 


,T 

1  u. 
n  1 


-LTjMuj+Uj 


Eq.  (68)  can  be  easily  expressed  in  terms  of  r.  and  <J.  By  multiplying  Eq.  (7) 
T  T  A 

on  the  left  by  T,  we  have  r  -  r.l  -(LT,)  1  .  Differentiating  this  equation 
twice  with  regard  to  t  and  substituting  it  into  the  first  equation  of  Eq.  (68) 


we  obtain 
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■  r'-l*M(LT  )T«  -  lV  ♦  1*M(LT  )T1  (69) 

in  i  ni  n  z  n 

Using  Eq.  (19)  we  csn  express  r  ss 

A 

T  T  •  T 

r  -  EUl-(T2p)  I (L)  u  *  (L)  ln] 


Substituting  the  above  equation  into  the  second  equation  of  Eq.  (68)  we  obtain 
Eq.  (28).  Combining  Eq.  (69)  and  Eq.  (28)  into  one  matrix  equation  we  have 


(70) 


These  are  the  equations  of  motion  for  the  system  with  tree  structure.  To  obtain 
the  equations  for  the  original  system  we  notice  that  we  have  some  further 
constraints  in  the  form  of  Eq.  (66).  Differentiating  this  equation  twice  with 

respect  to  time  and  substituting  the  expression  for  r  in  terms  of  r  and  we 
have  1 

C*T[r  1  -  (LT  )Ti  -  (LT  )T1  ]+(L*)T«  -  -(L*)T1  (71) 

l  n  L  2  n  n 


where  the  starred  quantities  have  the  same  meanings  as  the  unstarred  ones.  By 
using  the  notation  q  -  (r^.^1)1,  HT  -  (C*Tln,(L*)  -C*T(LT^T  and  Uj-C^lLTj)1^ 


.  *.T 

-a  >  io, 


Eq.  (71)  can  be  rewritten  as 


(72) 


Introducing  the  vector  Lagrange  multiplier  X*  the  equations  of  motion  can  be 
obtained  as  a  solution  of  the  following  matrix  equation: 


where  J  and  u  respectively  denote  the  matrix  and  the  right  hand  side  of  Eq. 

(70).  As  the  form  of  Eq.  (73)  is  the  same  as  before,  any  of  the  previously 
mentioned  method  can  be  used  for  its  solution.  The  exception,  of  course,  is 
when  the  rank  of  H  is  not  3n*  implying  that  the  constraints  given  by  Eq.  (72) 
are  not  independent.  We  must  therefore  remove  the  redundant  constraints  from 
Eq.  (72)  first  and  then  apply  the  methods  of  section  3  for  formulating  the 
equations  of  motion.  Alternatively  we  can  solve  Eq.  (73)  by  using  those  methods 
that  do  not  require  that  matrix  H  be  of  full  rank  3n*. 
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5 .  COMCLOSIOHS 

Several  computer-oriented  methods  for  the  formulation  of  the 
equations  of  motion  for  multibody  systems  have  been  presented  in  a  unified 
manner.  Starting  with  the  case  of  a  multibody  system  with  tree  structure  and 
with  ball-and-socket  joints,  and  using  Newton-Euler 's  method  it  became  evident 
that  the  equations  of  motion  written  in  the  vector-matrix  form  can  be  viewed  as 
the  stationary  point  condition  of  a  quadratic  programming  problem  with  equality 
constraints.  In  fact,  this  QP  problem  is  a  statement  of  a  not  too  well-known 
principle  called  the  Gauss's  principle  of  least  constraint  which  is  applicable 
to  both  holonomic  and  nonholonomic  systems.  Using  QP  theory  it  is  shown  that 
the  various  methods  are  nothing  else  but  different  ways  of  solving  the 
Lagrangian  system  of  equations  in  the  Lagrangian  method.  Procedures  are  also 
discussed  for  systems  with  joints  other  than  ball-and-socket  joints  and  for 
systems  with  closed  chains.  However,  due  to  the  lack  of  computational  results, 
no  comparisons  among  the  various  methods  are  made. 
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APPENDIX 


In  this  Appendix  we  shell  consider  the  solutions  of  systems  of 
equations  with  matrices  of  the  following  two  forms: 

T  -1 

H  J  H  (A-l ) 

T 

Z  J  Z  (A-2) 


where  H  and  Z  respectively  are  M  x  N  and  M  x  M-K  matrices  such  that 
ZTH*0,  K  is  the  rank  of  matrix  H,  and  J  is  an  MxM  positive  definite  matrix. 

The  symmetric  matrices  (A-l)  and  (A-2)  arise  in  the  formulation  of  the  equations 
of  motion  of  multibody  system.  For  example,  in  Eq.  (14) 


and  in  Eq.  (40-c) 


We  need  to  consider  two  cases  -(1)  rank  of  H  is  N  and  (2)  rank  of 

H  <  N. 


Case  1,  K-N 

In  this  case  the  matrix  Htj”*  H  is  positive  definite  and  the  usual 
method  for  solving  a  system  of  equations  with  symmetric  positive  definite  matrix 
is  the  Cholesky  factorization  with  sparsity  taken  into  consideration  [16-19]. 

Let  J  be  factored  as  J  ■  IJ)LT  where  L  is  a  unit  lower  triangular  matrix  and  D 
is  a  diagonal  matrix  with  positive  diagonal  elements.  Substituting  this  value 
of  J  into  Ht  J-^H  we  obtain 

T  -1  T  T  -1 
H  J  H  -  H  (LDL  )  H 

T  — T  — i  —1  (A-3) 

-  H  L  D  D  L  H 


The  Cholesky  f 
the  matrix  D“* 


actorization  is  now  obtained  by  performing  the  QR  factorization  of 
L_1H: 


where  Q  is  an  MxM  orthogonal  matrix  and  R  is  an  NxN  upper  triangular  matrix. 
Substituting  Eq.  (A-4)  into  (A-3)  we  obtain 

T  -1  T  T  /  R  \  T 

HJ  H  -  (R  0)QQl0J«RR,  (A-5) 

T  -1 

which  is  the  required  Cholesky  factorization  of  H  J  H. 


If  Z  is  known  Che  above  procedure  can  be  applied  Co  Che  facCorizacion 
of  ZTJZ.  However  if  Z  is  noC  known  we  firsC  perform  Che  QR  factor izat ion  of  H 
Co  decermine  Z: 

»-« l(oJ-  «• 

where  Q1  is  an  MxM  orChogonal  matrix  and  R  is  an  NxN  upper  triangular  matrix. 

Qj  is  partitioned  into  two  matrices  Q  and  Q*  such  Chat  Q  is  MxN  and  Q'  is 
MxM-N.  Since  Q,TQ*0  (Qa  is  orthogonal)  it  follows  that  Q,TH“0  and  hence 
Z*Q' .  It  should  be  noted  that  the  general  solution  to  the  equation  H^x*b  is 
given  by 

x  -  Yb  +  Zx'  (A-6) 

where  Y*QR~T  and  x*  is  any  (M-N)  vector.  This  can  be  easily  seen  by  noting 
that  Y  is  the  Moore-Penrose  generalized  inverse  of  H^.  However  we  mention 
that  for  the  solution  x  to  be  written  in  the  form  of  Eq.  (A-6)  Y  need  not  be  the 
generalized  inverse  of  HT.  All  that  is  required  is  to  find  Y  such  that 
HTY»EN.  Yt  can  be  regarded  as  the  left  generalized  inverse  of  H. 

A  general  method  [13]  for  finding  the  matrices  Y  and  Z  is  to  augment 
the  MxN  matrix  H  (M>N)  by  adding  an  MxM-N  matrix  H'  such  that  (H' ,H)  is 
nonsingular.  From  the  definition  of  the  inverse  it  follows  that 

(Z,Y)T  -  (H'.H)"1 

-T 

Let  H'^Q'.  Then  by  taking  H“QR  it  is  easy  to  check  that  Y=QR 
and  Z-Q' .  These  are  the  same  expressions  as  obtained  above  by  the  QR 
method.  To  obtain  some  other  expressions  for  Y  and  Z  let  us  assume  that  the 
last  N  rows  of  H  denoted  by  H2  are  linearly  independent.  Then  by  taking 
H’-<Em-N’°>T  we  have 

“l  j 1  _  /  “h-r  -"i^1 

H2/  \  0  H^1 

which  yields  Z^tE^^.-HjH^ 

Case  2,  K<;  N 

An  MxN  matrix  H, 

H  -  VjS jW*  (A-7 ) 

where  is  an  MxM  orthogonal  matrix,  is  an  NxN  orthogonal  matrix  and 
is  an  MxN  matrix  given  by 

Sl-(oS)  <*-«> 

with  S  ■  diag  {s^,...,s^}.  The  positive  real  numbers  s^,  i*l,...,K  are  the 
singular  values  of  the  matrix  H.  They  are  often  ordered  so  that 


j)  and  YT-(0,  H"1). 

M  >_  N,  and  of  rank  K  can  be  decomposed  as 


-28- 


•  ^  _>  • j •••  _>  •r>®*  ®y  partitioning  and  so  that  V^*(V,V')  and 
Wj“(W,W* )  and  substituting  into  Eq.  (A-7)  we  obtain 

H  -  V  S  WT  (A-9) 

where  VTV»E^  and  W^W  “  E^.  The  factorisat  ion  (A-7)  or  (A-9)  is  called  the 

singular  value  decoaiposition  [17].  The  general  solution  to  the  equation 
HTx-b,  if  exists,  is  given  by 

x  -  Yb  ♦  Zx*  (A- 10) 

-1  T 

where  Y  »  V  S  W  and  Z  -  V' .  It  should  be  noted  that  Y  is  the  Moore-Penrose 
generalized  inverse  of  H^. 


The  singular  value  decomposition  method  can  also  be  used  when  K**N. 
However,  because  of  the  efficiency  of  the  QR  decomposition  for  the  case  KSN  the 
singular  value  decomposition  should  be  used  only  when  K<N  or  when  it  cannot  be 
established  a  priori  that  K*N. 

T  -I 

As  in  the  case  when  rank  of  H  is  N  we  write  H  J  H  in  the  forj  o£  Eq. 

(A-3).  However  instead  of  taking  the  QR  factorization  of  the  matrix  D  L  H  we 
write  the  singular  value  decomposition: 

-A  -1  T 

D  *L  H  -  V  S  W4 


With  this  value,  the  matrix  H  J  H  can  be  written  as 

T  *1  T  T 

H  J  *H*»S  V  VSW 

2  T 

-  W  S  W 

T  -1 

and  a  solution  to  the  matrix  equation  H  J  Hx  *  u  is 

-2  T 
x  -  W  S  W  u 


(  A- 1 1 ) 


(  A- 1 2  ) 


It  is  of  course  assumed  that  the  constraint  equations  are  consistent  so  that  a 
solution  to  the  matrix  equation  exists.  ^ 

For  the  factorization  of  the  matrix  Z  JZ  we  first  determine  Z  of  full 
rank  M-K  by  the  singular  value  decomposition  and  then  write  the  Cholesky 
factorization  of  ZTJZ  by  the  QR  method. 
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